<!DOCTYPE html>
<html>
<head>
<meta charset="UTF-8">
<title>KalmanFilter</title>
<script type="text/x-mathjax-config">
MathJax.Hub.Config({
  tex2jax: { inlineMath: [['$','$'], ['\\(','\\)']] },
  TeX: { equationNumbers: { autoNumber: 'all' } }
});
</script>
<script type="text/javascript" async src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-MML-AM_CHTML"></script>
<style>
body > * {
  max-width: 1000px;
}
body {
  font-family: "Roboto Condensed", sans-serif;
  padding-left: 7.5em;
  padding-right: 7.5em;
}
pre, code {
  max-width: 60em;
  font-family: monospace;
}
pre.oct-code {
  border: 1px solid Grey;
  padding: 5px;
}
pre.oct-code-output {
  margin-left: 2em;
}
span.comment {
  color: ForestGreen;
}
span.keyword {
  color: Blue;
}
span.string {
  color: DarkOrchid;
}
footer {
  margin-top: 2em;
  font-size: 80%;
}
a, a:visited {
  color: Blue;
}
h2 {
  font-family: "Roboto Condensed", serif;
  margin-top: 1.5em;
}
h2 a, h2 a:visited {
  color: Black;
}
</style>
</head>
<body>
<pre class="oct-code"><span class="comment">% Defining the tire and nonlinear vehicle models with the default parameters</span>
<span class="comment">%</span>

TirePlant = TirePacejka;
VehiclePlant = VehicleSimpleNonlinear;
VehiclePlant.tire = TirePlant;</pre><p><b>Maneuver</b></p><p>This section presents the vehicle maneuver to be estimated by the Kalman Filter.</p><p>Choosing simulation parameters:</p><pre class="oct-code">T = 6;                              <span class="comment">% Total simulation time [s]</span>
resol = 50;                         <span class="comment">% Resolution</span>
TSPAN = 0:T/resol:T;                <span class="comment">% Time span [s]</span>
</pre><p>Initializing the simulator and simulating.</p><pre class="oct-code">simulatorPlant = Simulator(VehiclePlant, TSPAN);
simulatorPlant.dPSI0 = 0.35;
simulatorPlant.Simulate</pre><p>Retrieving state responses</p><pre class="oct-code">XTPlant = simulatorPlant.XT;
YTPlant = simulatorPlant.YT;
PSIPlant = simulatorPlant.PSI;
vTPlant = simulatorPlant.VEL;
ALPHATPlant = simulatorPlant.ALPHAT;
dPSIPlant = simulatorPlant.dPSI;

XOUTPlant = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];</pre><p>Generating the graphics of the maneuver.</p><pre class="oct-code">gPlant = Graphics(simulatorPlant);
gPlant.TractorColor = <span class="string">'r'</span>;
gPlant.Frame();</pre><img width="800" src="../illustrations/frame/KalmanFilterFrame1.svg" alt="../illustrations/frame/KalmanFilterFrame1.svg"><p><b>Kalman Filter Model</b></p><p>Initializing the tire model</p><pre class="oct-code">TireModel = TireLinear;</pre><p>Choosing model vehicle</p><pre class="oct-code">VehicleModel = VehicleSimpleNonlinear;
VehicleModel.tire = TireModel;</pre><p>Simulating with the same time span.</p><pre class="oct-code">simulatorModel = Simulator(VehicleModel, TSPAN);
simulatorModel.dPSI0 = 0.35;
simulatorModel.Simulate;</pre><p>Retrieving states</p><pre class="oct-code">XTModel = simulatorModel.XT;
YTModel = simulatorModel.YT;
PSIModel = simulatorModel.PSI;
vTModel = simulatorModel.VEL;
ALPHATModel = simulatorModel.ALPHAT;
dPSIModel = simulatorModel.dPSI;</pre><p>The maneuver generated by the simplified model (with the same initial conditions) is illustrated below</p><pre class="oct-code">gModel = Graphics(simulatorModel);
gModel.TractorColor = <span class="string">'g'</span>;
gModel.Frame();</pre><img width="800" src="../illustrations/frame/KalmanFilterFrame2.svg" alt="../illustrations/frame/KalmanFilterFrame2.svg"><p><b>Plant and model comparison</b></p><p>Comparing tire models</p><pre class="oct-code">g = 9.81;
FzF = VehiclePlant.mF0*g;
FzR = VehiclePlant.mR0*g;
muy = VehiclePlant.muy;
nF = VehiclePlant.nF;
nR = VehiclePlant.nR;

alpha= 0:0.5:15;
alpha = alpha*pi/180;
FyLin = - TireModel.Characteristic(alpha);
FyFPac = - TirePlant.Characteristic(alpha, FzF, muy);
FyRPac = - TirePlant.Characteristic(alpha, FzR, muy);

figure
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(alpha(1:floor(end/2))*180/pi,FyLin(1:floor(end/2)),<span class="string">'r'</span>)
plot(alpha*180/pi,FyFPac,<span class="string">'g'</span>)
plot(alpha*180/pi,FyRPac,<span class="string">'g--'</span>)
xlabel(<span class="string">'alpha [deg]'</span>)
ylabel(<span class="string">'Fy [N]'</span>)
l = legend(<span class="string">'Linear'</span>,<span class="string">'Pacejka F'</span>,<span class="string">'Pacejka R'</span>);
set(l,<span class="string">'Location'</span>,<span class="string">'SouthEast'</span>)</pre><p>Comparing state response</p><pre class="oct-code">f1 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,XTPlant,<span class="string">'r'</span>)
plot(TSPAN,XTModel,<span class="string">'r--'</span>)
legend(<span class="string">'Plant'</span>,<span class="string">'Model'</span>)
xlabel(<span class="string">'Time [s]'</span>)
ylabel(<span class="string">'Distance X [m]'</span>)

f2 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,YTPlant,<span class="string">'g'</span>)
plot(TSPAN,YTModel,<span class="string">'g--'</span>)
legend(<span class="string">'Plant'</span>,<span class="string">'Model'</span>)
xlabel(<span class="string">'Time [s]'</span>)
ylabel(<span class="string">'Distance Y [m]'</span>)

f3 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,PSIPlant,<span class="string">'b'</span>)
plot(TSPAN,PSIModel,<span class="string">'b--'</span>)
legend(<span class="string">'Plant'</span>,<span class="string">'Model'</span>)
xlabel(<span class="string">'Time [s]'</span>)
ylabel(<span class="string">'PSI [rad]'</span>)

f4 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,vTPlant,<span class="string">'c'</span>)
plot(TSPAN,vTModel,<span class="string">'c--'</span>)
legend(<span class="string">'Plant'</span>,<span class="string">'Model'</span>)
xlabel(<span class="string">'Time [s]'</span>)
ylabel(<span class="string">'vT [m/s]'</span>)

f5 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,ALPHATPlant,<span class="string">'m'</span>),
plot(TSPAN,ALPHATModel,<span class="string">'m--'</span>),
legend(<span class="string">'Plant'</span>,<span class="string">'Model'</span>)
xlabel(<span class="string">'Time [s]'</span>)
ylabel(<span class="string">'ALPHAT [rad/s]'</span>)

f6 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,dPSIPlant,<span class="string">'k'</span>)
plot(TSPAN,dPSIModel,<span class="string">'k--'</span>)
legend(<span class="string">'Plant'</span>,<span class="string">'Model'</span>)
xlabel(<span class="string">'Time [s]'</span>)
ylabel(<span class="string">'dPSI [rad/s]'</span>)</pre><img width="500" src="../illustrations/plot/KalmanFilterFig1.svg" alt="../illustrations/plot/KalmanFilterFig1.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig2.svg" alt="../illustrations/plot/KalmanFilterFig2.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig3.svg" alt="../illustrations/plot/KalmanFilterFig3.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig4.svg" alt="../illustrations/plot/KalmanFilterFig4.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig5.svg" alt="../illustrations/plot/KalmanFilterFig5.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig6.svg" alt="../illustrations/plot/KalmanFilterFig6.svg"><p>Comparing longitudinal and lateral acceleration</p><pre class="oct-code">saidasPlant = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];
matDerivEstadosPlant = zeros(size(saidasPlant));
<span class="keyword">for</span> i = 1:size(saidasPlant,1)
    auxil = simulatorPlant.Vehicle.Model(1,saidasPlant(i,:),TSPAN);
    matDerivEstadosPlant(i,:) = auxil<span class="string">';</span>
<span class="keyword">end</span>

dXTPlant = matDerivEstadosPlant(:,1);
dYTPlant = matDerivEstadosPlant(:,2);
dPSIPlant = matDerivEstadosPlant(:,3);
dvTPlant = matDerivEstadosPlant(:,4);
dALPHATPlant = matDerivEstadosPlant(:,5);
ddPSIPlant = matDerivEstadosPlant(:,6);

ddXPlant = dvTPlant.*cos(PSIPlant + ALPHATPlant) - vTPlant.*(dPSIPlant + dALPHATPlant).*sin(PSIPlant + ALPHATPlant);
ddYPlant = dvTPlant.*sin(PSIPlant + ALPHATPlant) + vTPlant.*(dPSIPlant + dALPHATPlant).*cos(PSIPlant + ALPHATPlant);

ACELNumPlant = [(ddXPlant.*cos(PSIPlant) - ddYPlant.*sin(PSIPlant))  (-ddXPlant.*sin(PSIPlant) + ddYPlant.*cos(PSIPlant))];

saidasModel = [XTModel YTModel PSIModel vTModel ALPHATModel dPSIModel];
matDerivEstadosModel = zeros(size(saidasModel));
<span class="keyword">for</span> i = 1:size(saidasModel,1)
    auxil = simulatorModel.Vehicle.Model(1,saidasModel(i,:),TSPAN);
    matDerivEstadosModel(i,:) = auxil<span class="string">';</span>
<span class="keyword">end</span>

dXTModel = matDerivEstadosModel(:,1);
dYTModel = matDerivEstadosModel(:,2);
dPSIModel = matDerivEstadosModel(:,3);
dvTModel = matDerivEstadosModel(:,4);
dALPHATModel = matDerivEstadosModel(:,5);
ddPSIModel = matDerivEstadosModel(:,6);

ddXModel = dvTModel.*cos(PSIModel + ALPHATModel) - vTModel.*(dPSIModel + dALPHATModel).*sin(PSIModel + ALPHATModel);
ddYModel = dvTModel.*sin(PSIModel + ALPHATModel) + vTModel.*(dPSIModel + dALPHATModel).*cos(PSIModel + ALPHATModel);

ACELNumModel = [(ddXModel.*cos(PSIModel) - ddYModel.*sin(PSIModel))  (-ddXModel.*sin(PSIModel) + ddYModel.*cos(PSIModel))];

f7 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,ACELNumPlant(:,1),<span class="string">'r'</span>)
plot(TSPAN,ACELNumPlant(:,2),<span class="string">'g'</span>)
plot(TSPAN,ACELNumModel(:,1),<span class="string">'r--'</span>)
plot(TSPAN,ACELNumModel(:,2),<span class="string">'g--'</span>)
xlabel(<span class="string">'time [s]'</span>)
ylabel(<span class="string">'acc. [m/s]'</span>)
l = legend(<span class="string">'AX Plant'</span>,<span class="string">'AY Plant'</span>,<span class="string">'AX Model'</span>,<span class="string">'AY Model'</span>);
set(l,<span class="string">'Location'</span>,<span class="string">'NorthEast'</span>)</pre><img width="500" src="../illustrations/plot/KalmanFilterFig7.svg" alt="../illustrations/plot/KalmanFilterFig7.svg"><p>Comparing the maneuvers of the plant and the model.</p><pre class="oct-code">set(gcf,<span class="string">'nextplot'</span>,<span class="string">'replace'</span>)
gPlant.Frame();
hold on
gModel.Frame();</pre><img width="800" src="../illustrations/frame/KalmanFilterFrame3.svg" alt="../illustrations/frame/KalmanFilterFrame3.svg"><p><b>Model linearization</b></p><p>The general expression of the linearized model is optained using the symbolic processor of Octave/Matlab.</p><p>Defining symbols</p><pre class="oct-code">syms XT YT PSI vT ALPHAT dPSI mT IT a b K</pre><p>Slip angles</p><pre class="oct-code">ALPHAF = atan((vT * sin(ALPHAT) + a * dPSI)/(vT * cos(ALPHAT))); <span class="comment">% Dianteiro</span>
ALPHAR = atan((vT * sin(ALPHAT) - b * dPSI)/(vT * cos(ALPHAT))); <span class="comment">% Traseiro</span>
</pre><p>Lateral forces</p><pre class="oct-code">FyF = -K*ALPHAF;
FyR = -K*ALPHAR;</pre><p>State equations</p><pre class="oct-code">f1e = vT * cos(ALPHAT + PSI);
f2e = vT * sin(ALPHAT + PSI);
f3e = dPSI;
f4e = (FyF * sin(ALPHAT) + FyR * sin(ALPHAT))/(mT);
f5e = (FyF * cos(ALPHAT) + FyR * cos(ALPHAT) - mT * vT * dPSI) / (mT * vT);
f6e = (FyF * a - FyR * b) / IT;

f = [f1e ; f2e ; f3e ; f4e ; f5e ; f6e];</pre><p>State vector</p><pre class="oct-code">States = [XT ; YT ; PSI ; vT ; ALPHAT ; dPSI];</pre><p>The linearized system is written as</p><p>$$ \dot{\bf x} = {\bf F} {\bf x} $$</p><p>where ${\bf F}$ is the dynamic matrix of the linear model obtained after the truncation of the expansion of the Taylor series. Thus, the ${\bf F}$ matrix is</p><p>$$ {\bf F} = \left[ \frac{\partial f_i}{\partial x_j} \right]_{n \times n} $$</p><p>where $i$ e $j$ indicate the equations and state variables used in the calculations of at the position $(i,j)$ of the jacobian matrix.</p><p>Calculating the jacobian matrix.</p><pre class="oct-code">F = jacobian(f,States);
F = simplify(F);</pre><p>Defining a matlab function to retrieve the numerical value of F. This way is faster than using "subs".</p><pre class="oct-code">Ffunc = matlabFunction(F,<span class="string">'Vars'</span>,{PSI vT ALPHAT dPSI mT IT a b K});</pre><p><b>Measurements</b></p><p>Hypothetically, the measured quantities are:</p><ul><li>Position X</li><li>Position Y</li><li>Yaw rate</li><li>Longitudinal acceleration</li><li>Lateral acceleration</li></ul><p>For this, we use:</p><p>$$ \dot{x} = v_{\rm T} \cos \left( \psi + \alpha_{\rm T} \right) $$</p><p>$$ \dot{y} = v_{\rm T} \sin \left( \psi + \alpha_{\rm T} \right) $$</p><p>Longitudinal and lateral acceleration are</p><p>$$ \ddot{x} = \dot{v}_{\rm T} \cos \left( \psi + \alpha_{\rm T} \right) - v_{\rm T} \left( \dot{\psi} + \dot{\alpha}_{\rm T} \right) \sin \left( \psi + \alpha_{\rm T} \right) $$</p><p>$$ \ddot{y} = \dot{v}_{\rm T} \sin \left( \psi + \alpha_{\rm T} \right) + v_{\rm T} \left( \dot{\psi} + \dot{\alpha}_{\rm T} \right) \cos \left( \psi + \alpha_{\rm T} \right) $$</p><p>Thus</p><pre class="oct-code">ddX = f4*cos(PSI + ALPHAT) - vT*(dPSI + f5)*sin(PSI + ALPHAT);
ddY = f4*sin(PSI + ALPHAT) + vT*(dPSI + f5)*cos(PSI + ALPHAT);</pre><p>The equations above describe the dynamics from $ \{ O {\bf i} {\bf j} {\bf k} \} $. In the moving reference frame $ \{ O {\bf t}_x {\bf t}_y {\bf t}_z \} $ the equations are</p><p>$$ {\bf a} = \left( \ddot{x} \cos \psi - \ddot{y} \sin \psi \right) {\bf t}_x + \left( - \ddot{x} \sin \psi + \ddot{y} \sin \psi \right) {\bf t}_y $$</p><p>Thus,</p><pre class="oct-code">ACEL = [ddX*cos(PSI) - ddY*sin(PSI) ; -ddX*sin(PSI) + ddY*cos(PSI)];
ACEL = simplify(ACEL);</pre><p>The nonlinear observation equation is given by</p><p>$$ {\bf z}_k = {\bf h} ({\bf x}_k) + {\bf v}_k $$</p><p>with $ {\bf v}_k $ ~ $ N ( {\bf 0} , {\bf R}_k ) $.</p><p>The linear version is given by</p><p>$$ {\bf z}_k = {\bf H} {\bf x}_k + {\bf v}_k $$</p><p>where</p><p>$$ {\bf H} = \left[ \frac{\partial h_i}{\partial x_j} \right]_{m \times n} $$</p><p>that is, the output matrix {\bf H} is the jacobian matrix of the ACEL equation in relation to the system states.</p><pre class="oct-code">medNonlinear = [XT ; YT ; f6 ; ACEL];

H = jacobian(medNonlinear,States);
H = simplify(H);</pre><p>Again, defining matlab functions to retrieve the numerical value of medNonlinearfunc and H.</p><pre class="oct-code">medNonlinearfunc = matlabFunction(medNonlinear,<span class="string">'Vars'</span>,{XT YT PSI vT ALPHAT dPSI mT IT a b K});

Hfunc = matlabFunction(H,<span class="string">'Vars'</span>,{PSI vT ALPHAT dPSI mT IT a b K});</pre><p><b>Extended Kalman Filter</b></p><p>Implementing the algorithm</p><p>Noise matrix</p><pre class="oct-code">G = eye(6); <span class="comment">% Matriz identidade (6 x 6)</span>
</pre><p>Covariance matrices</p><pre class="oct-code">Q = eye(6);
R = eye(5);</pre><p>Matrix</p><pre class="oct-code">P0 = eye(6);</pre><p>Retrieving the initial conditions defined above.</p><pre class="oct-code">X0Num = simulatorModel.X0;
Y0Num = simulatorModel.Y0;
PSI0Num = simulatorModel.PSI0;
VEL0Num = simulatorModel.V0;
ALPHAT0Num = simulatorModel.ALPHAT0;
dPSI0Num = simulatorModel.dPSI0;

x0 = [ X0Num ; Y0Num ; PSI0Num ; VEL0Num ; ALPHAT0Num ; dPSI0Num ];
x0 = zeros(6,1);
x0(4)=20;</pre><p>Retrieving the vehicle parameters.</p><pre class="oct-code">mTNum = VehicleModel.mT;
ITNum = VehicleModel.IT;
aNum = VehicleModel.a;
bNum = VehicleModel.b;
KNum = TireModel.k;

parameters = [mTNum ITNum aNum bNum KNum];</pre><p>Sample time</p><pre class="oct-code">sampleTime = 0.1;

t = 0:sampleTime:T;                 <span class="comment">% Array of observation instants</span>
</pre><p>Preallocation</p><pre class="oct-code">XOUTopt = zeros(length(t) + 1,length(States));  <span class="comment">% Estimate of the states after update</span>
Popt = zeros(length(t) + 1,1);                  <span class="comment">% Covariance matrix after update</span>
Pantes = zeros(length(t) + 1,1);                <span class="comment">% Covariance matrix before update</span>
KKalmanopt = ones(length(t) + 1,1);             <span class="comment">% Kalman gain</span>
</pre><p>Using the first values</p><pre class="oct-code">XOUTopt(1,:) = x0<span class="string">';</span>
XOUTantes(1,:) = x0<span class="string">';</span>
Pantes(1,1) = norm(P0);
Popt(1,1) = norm(P0);

<span class="comment">% Error distribution</span>

pesos = [5; 5; 0.1; 0.5; 0.5];</pre><p><b>Iterations</b></p><pre class="oct-code"><span class="keyword">for</span> j = 1:length(t)
    <span class="comment">% Index varying through all instants of observation</span>

    <span class="comment">% Time span for the propagation phase</span>
    tspan = t(j):sampleTime/100:t(j)+sampleTime;

    <span class="comment">% Measures of the interation</span>
    z = [interp1(TSPAN,XTPlant(:,1),t(j)) ;
         interp1(TSPAN,YTPlant(:,1),t(j)) ;
         interp1(TSPAN,ddPSIPlant(:,1),t(j)) ;
         interp1(TSPAN,ACELNumPlant(:,1),t(j)) ;
         interp1(TSPAN,ACELNumPlant(:,2),t(j))];

    z = z + pesos.*(rand(5,1)-0.5);

    Fnum = Ffunc(PSI0Num,VEL0Num,ALPHAT0Num,dPSI0Num,mTNum,ITNum,aNum,bNum,KNum);
    Hnum = Hfunc(PSI0Num,VEL0Num,ALPHAT0Num,dPSI0Num,mTNum,ITNum,aNum,bNum,KNum);

    <span class="comment">% Propagation cycle</span>
    <span class="comment">% Transforming matrix PMat0 (6 x 6) in P0 (1 x 36)</span>
    P0 = reshape(P0<span class="string">',[1 36]);</span>

    [TOUT,Pout] = ode45(@(t,P) IntCov(t,P,Fnum,G,Q),tspan,P0);

    Pmatrix = reshape(Pout(end,:),[6 6])<span class="string">';</span>

    simulatorKalman = Simulator(VehicleModel, tspan);

    <span class="comment">% Initial conditions</span>
    simulatorKalman.X0 = x0(1);
    simulatorKalman.Y0 = x0(2);
    simulatorKalman.PSI0 = x0(3);
    simulatorKalman.V0 = x0(4);
    simulatorKalman.ALPHAT0 = x0(5);
    simulatorKalman.dPSI0 = x0(6);

    <span class="comment">% Simulation</span>
    simulatorKalman.Simulate()

    XTKalman = simulatorKalman.XT;
    YTKalman = simulatorKalman.YT;
    PSIKalman = simulatorKalman.PSI;
    vTKalman = simulatorKalman.VEL;
    ALPHATKalman = simulatorKalman.ALPHAT;
    dPSIKalman = simulatorKalman.dPSI;

    XOUTKalman = [XTKalman YTKalman PSIKalman vTKalman ALPHATKalman dPSIKalman];

    <span class="comment">% Update cycle</span>

    ACELKalman = medNonlinearfunc(XTKalman(end),
                                  YTKalman(end),
                                  PSIKalman(end),
                                  vTKalman(end),
                                  ALPHATKalman(end),
                                  dPSIKalman(end),
                                  mTNum,
                                  ITNum,
                                  aNum,
                                  bNum,
                                  KNum);

    KKalman = Pmatrix*Hnum<span class="string">' / (Hnum*Pmatrix*Hnum'</span> + R);

    XKalman = XOUTKalman(end,:)<span class="string">' + KKalman*(z - ACELKalman);</span>
    PKalman = Pmatrix - KKalman*Hnum*Pmatrix;

    x0 = XKalman;
    P0 = PKalman;

    XOUTopt(j+1,:) = XKalman<span class="string">';</span>
    XOUTantes(j+1,:) = XOUTKalman(end,:);
    Popt(j+1) = norm(PKalman);
    Pantes(j+1) = norm(Pmatrix);
    KKalmanopt(j+1) = norm(KKalman);

<span class="keyword">end</span></pre><p><b>Comparison</b></p><pre class="oct-code">f8 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,XOUTPlant(:,1),<span class="string">'r'</span>)
p = plot(t(2:end),XOUTopt(2:end-1,1),<span class="string">'r--'</span>);
set(p,<span class="string">'Marker'</span>,<span class="string">'*'</span>,<span class="string">'MarkerSize'</span>,3)
l = legend(<span class="string">'Plant'</span>,<span class="string">'Estim'</span>);
set(l,<span class="string">'Location'</span>,<span class="string">'SouthEast'</span>)
xlabel(<span class="string">'tempo [s]'</span>)
ylabel(<span class="string">'x [m]'</span>)

f9 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,XOUTPlant(:,2),<span class="string">'g'</span>)
p = plot(t(2:end),XOUTopt(2:end-1,2),<span class="string">'g--'</span>);
set(p,<span class="string">'Marker'</span>,<span class="string">'*'</span>,<span class="string">'MarkerSize'</span>,3)
l = legend(<span class="string">'Plant'</span>,<span class="string">'Estim'</span>);
set(l,<span class="string">'Location'</span>,<span class="string">'SouthEast'</span>)
xlabel(<span class="string">'tempo [s]'</span>)
ylabel(<span class="string">'y [m]'</span>)

f10 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,XOUTPlant(:,3),<span class="string">'b'</span>)
p = plot(t(2:end),XOUTopt(2:end-1,3),<span class="string">'b--'</span>);
set(p,<span class="string">'Marker'</span>,<span class="string">'*'</span>,<span class="string">'MarkerSize'</span>,3)
l = legend(<span class="string">'Plant'</span>,<span class="string">'Estim'</span>);
set(l,<span class="string">'Location'</span>,<span class="string">'SouthEast'</span>)
xlabel(<span class="string">'tempo [s]'</span>)
ylabel(<span class="string">'PSI [rad]'</span>)

f11 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,XOUTPlant(:,4),<span class="string">'c'</span>)
p = plot(t(2:end),XOUTopt(2:end-1,4),<span class="string">'c--'</span>);
set(p,<span class="string">'Marker'</span>,<span class="string">'*'</span>,<span class="string">'MarkerSize'</span>,3)
l = legend(<span class="string">'Plant'</span>,<span class="string">'Estim'</span>);
set(l,<span class="string">'Location'</span>,<span class="string">'SouthEast'</span>)
xlabel(<span class="string">'tempo [s]'</span>)
ylabel(<span class="string">'vT [m/s]'</span>)

f12 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,XOUTPlant(:,5),<span class="string">'m'</span>)
p = plot(t(2:end),XOUTopt(2:end-1,5),<span class="string">'m--'</span>);
set(p,<span class="string">'Marker'</span>,<span class="string">'*'</span>,<span class="string">'MarkerSize'</span>,3)
l = legend(<span class="string">'Plant'</span>,<span class="string">'Estim'</span>);
set(l,<span class="string">'Location'</span>,<span class="string">'SouthEast'</span>)
xlabel(<span class="string">'tempo [s]'</span>)
ylabel(<span class="string">'ALPHAT [rad/s]'</span>)

f13 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
plot(TSPAN,XOUTPlant(:,6),<span class="string">'k'</span>)
p = plot(t(2:end),XOUTopt(2:end-1,6),<span class="string">'k--'</span>);
set(p,<span class="string">'Marker'</span>,<span class="string">'*'</span>,<span class="string">'MarkerSize'</span>,3)
l = legend(<span class="string">'Plant'</span>,<span class="string">'Estim'</span>);
set(l,<span class="string">'Location'</span>,<span class="string">'SouthEast'</span>)
xlabel(<span class="string">'tempo [s]'</span>)
ylabel(<span class="string">'dPSI [rad/s]'</span>)

<span class="comment">% Cov. do erro</span>
f14 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
p = plot(t(2:end),KKalmanopt(2:end-1),<span class="string">'r'</span>);
set(p,<span class="string">'Marker'</span>,<span class="string">'*'</span>,<span class="string">'MarkerSize'</span>,3)
ylabel(<span class="string">'ganho de kalman'</span>)
xlabel(<span class="string">'tempo [s]'</span>)

f15 = figure;
ax = gca;
set(ax,<span class="string">'NextPlot'</span>,<span class="string">'add'</span>,<span class="string">'Box'</span>,<span class="string">'on'</span>,<span class="string">'XGrid'</span>,<span class="string">'on'</span>,<span class="string">'YGrid'</span>,<span class="string">'on'</span>)
p = plot(t(2:end),Popt(2:end-1),<span class="string">'r'</span>);
set(p,<span class="string">'Marker'</span>,<span class="string">'*'</span>,<span class="string">'MarkerSize'</span>,3)
p = plot(t(2:end),Pantes(2:end-1),<span class="string">'g'</span>);
set(p,<span class="string">'Marker'</span>,<span class="string">'*'</span>,<span class="string">'MarkerSize'</span>,3)
l = legend(<span class="string">'+'</span>,<span class="string">'-'</span>);
set(l,<span class="string">'Location'</span>,<span class="string">'NorthEast'</span>)
ylabel(<span class="string">'cov. erro'</span>)
xlabel(<span class="string">'tempo [s]'</span>)</pre><img width="500" src="../illustrations/plot/KalmanFilterFig8.svg" alt="../illustrations/plot/KalmanFilterFig8.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig9.svg" alt="../illustrations/plot/KalmanFilterFig9.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig10.svg" alt="../illustrations/plot/KalmanFilterFig10.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig11.svg" alt="../illustrations/plot/KalmanFilterFig11.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig12.svg" alt="../illustrations/plot/KalmanFilterFig12.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig13.svg" alt="../illustrations/plot/KalmanFilterFig13.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig14.svg" alt="../illustrations/plot/KalmanFilterFig14.svg"><img width="500" src="../illustrations/plot/KalmanFilterFig15.svg" alt="../illustrations/plot/KalmanFilterFig15.svg"><p><b>Estimated trajectory</b></p><p>Using simulatorPlant to initialize the Kalman frame.</p><pre class="oct-code">set(gcf,<span class="string">'nextplot'</span>,<span class="string">'replace'</span>)
gKalman = Graphics(simulatorKalman);
gKalman.Simulator.TSpan = t;
gKalman.Simulator.XT = XOUTopt(1:end-1,1);
gKalman.Simulator.YT = XOUTopt(1:end-1,2);
gKalman.Simulator.PSI = XOUTopt(1:end-1,3);
gKalman.Simulator.VEL = XOUTopt(1:end-1,4);
gKalman.Simulator.ALPHAT = XOUTopt(1:end-1,5);
gKalman.Simulator.dPSI = XOUTopt(1:end-1,6);
gKalman.TractorColor = <span class="string">'b'</span>;

set(gcf,<span class="string">'nextplot'</span>,<span class="string">'replace'</span>)
gPlant.Frame();
hold on
gKalman.Frame();</pre><img width="800" src="../illustrations/frame/KalmanFilterFrame4.svg" alt="../illustrations/frame/KalmanFilterFrame4.svg">
<footer><hr><a href="http://www.octave.org">Published with GNU Octave 4.2.1</a></footer>
<!--
##### SOURCE BEGIN #####
% Defining the tire and nonlinear vehicle models with the default parameters
%

TirePlant = TirePacejka;
VehiclePlant = VehicleSimpleNonlinear;
VehiclePlant.tire = TirePlant;

%%
% *Maneuver*
%
% This section presents the vehicle maneuver to be estimated by the Kalman Filter.
%
% Choosing simulation parameters:
%

T = 6;                              % Total simulation time [s]
resol = 50;                         % Resolution
TSPAN = 0:T/resol:T;                % Time span [s]

%%
% Initializing the simulator and simulating.
%

simulatorPlant = Simulator(VehiclePlant, TSPAN);
simulatorPlant.dPSI0 = 0.35;
simulatorPlant.Simulate

%%
% Retrieving state responses
%

XTPlant = simulatorPlant.XT;
YTPlant = simulatorPlant.YT;
PSIPlant = simulatorPlant.PSI;
vTPlant = simulatorPlant.VEL;
ALPHATPlant = simulatorPlant.ALPHAT;
dPSIPlant = simulatorPlant.dPSI;

XOUTPlant = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];

%%
% Generating the graphics of the maneuver.
%

gPlant = Graphics(simulatorPlant);
gPlant.TractorColor = 'r';
gPlant.Frame();

%%
% <<../illustrations/frame/KalmanFilterFrame1.svg>>
%
% *Kalman Filter Model*
%
% Initializing the tire model
%

TireModel = TireLinear;

%%
% Choosing model vehicle
%

VehicleModel = VehicleSimpleNonlinear;
VehicleModel.tire = TireModel;

%%
% Simulating with the same time span.
%

simulatorModel = Simulator(VehicleModel, TSPAN);
simulatorModel.dPSI0 = 0.35;
simulatorModel.Simulate;

%%
% Retrieving states
%

XTModel = simulatorModel.XT;
YTModel = simulatorModel.YT;
PSIModel = simulatorModel.PSI;
vTModel = simulatorModel.VEL;
ALPHATModel = simulatorModel.ALPHAT;
dPSIModel = simulatorModel.dPSI;

%%
% The maneuver generated by the simplified model (with the same initial conditions) is illustrated below
%

gModel = Graphics(simulatorModel);
gModel.TractorColor = 'g';
gModel.Frame();

%%
% <<../illustrations/frame/KalmanFilterFrame2.svg>>
%
% *Plant and model comparison*
%
% Comparing tire models
%

g = 9.81;
FzF = VehiclePlant.mF0*g;
FzR = VehiclePlant.mR0*g;
muy = VehiclePlant.muy;
nF = VehiclePlant.nF;
nR = VehiclePlant.nR;

alpha= 0:0.5:15;
alpha = alpha*pi/180;
FyLin = - TireModel.Characteristic(alpha);
FyFPac = - TirePlant.Characteristic(alpha, FzF, muy);
FyRPac = - TirePlant.Characteristic(alpha, FzR, muy);

figure
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(alpha(1:floor(end/2))*180/pi,FyLin(1:floor(end/2)),'r')
plot(alpha*180/pi,FyFPac,'g')
plot(alpha*180/pi,FyRPac,'g--')
xlabel('alpha [deg]')
ylabel('Fy [N]')
l = legend('Linear','Pacejka F','Pacejka R');
set(l,'Location','SouthEast')

%%
% Comparing state response
%

f1 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XTPlant,'r')
plot(TSPAN,XTModel,'r--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('Distance X [m]')

f2 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,YTPlant,'g')
plot(TSPAN,YTModel,'g--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('Distance Y [m]')

f3 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,PSIPlant,'b')
plot(TSPAN,PSIModel,'b--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('PSI [rad]')

f4 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,vTPlant,'c')
plot(TSPAN,vTModel,'c--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('vT [m/s]')

f5 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,ALPHATPlant,'m'),
plot(TSPAN,ALPHATModel,'m--'),
legend('Plant','Model')
xlabel('Time [s]')
ylabel('ALPHAT [rad/s]')

f6 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,dPSIPlant,'k')
plot(TSPAN,dPSIModel,'k--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('dPSI [rad/s]')

%%
% <<../illustrations/plot/KalmanFilterFig1.svg>>
%
% <<../illustrations/plot/KalmanFilterFig2.svg>>
%
% <<../illustrations/plot/KalmanFilterFig3.svg>>
%
% <<../illustrations/plot/KalmanFilterFig4.svg>>
%
% <<../illustrations/plot/KalmanFilterFig5.svg>>
%
% <<../illustrations/plot/KalmanFilterFig6.svg>>
%
% Comparing longitudinal and lateral acceleration
%

saidasPlant = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];
matDerivEstadosPlant = zeros(size(saidasPlant));
for i = 1:size(saidasPlant,1)
    auxil = simulatorPlant.Vehicle.Model(1,saidasPlant(i,:),TSPAN);
    matDerivEstadosPlant(i,:) = auxil';
end

dXTPlant = matDerivEstadosPlant(:,1);
dYTPlant = matDerivEstadosPlant(:,2);
dPSIPlant = matDerivEstadosPlant(:,3);
dvTPlant = matDerivEstadosPlant(:,4);
dALPHATPlant = matDerivEstadosPlant(:,5);
ddPSIPlant = matDerivEstadosPlant(:,6);

ddXPlant = dvTPlant.*cos(PSIPlant + ALPHATPlant) - vTPlant.*(dPSIPlant + dALPHATPlant).*sin(PSIPlant + ALPHATPlant);
ddYPlant = dvTPlant.*sin(PSIPlant + ALPHATPlant) + vTPlant.*(dPSIPlant + dALPHATPlant).*cos(PSIPlant + ALPHATPlant);

ACELNumPlant = [(ddXPlant.*cos(PSIPlant) - ddYPlant.*sin(PSIPlant))  (-ddXPlant.*sin(PSIPlant) + ddYPlant.*cos(PSIPlant))];

saidasModel = [XTModel YTModel PSIModel vTModel ALPHATModel dPSIModel];
matDerivEstadosModel = zeros(size(saidasModel));
for i = 1:size(saidasModel,1)
    auxil = simulatorModel.Vehicle.Model(1,saidasModel(i,:),TSPAN);
    matDerivEstadosModel(i,:) = auxil';
end

dXTModel = matDerivEstadosModel(:,1);
dYTModel = matDerivEstadosModel(:,2);
dPSIModel = matDerivEstadosModel(:,3);
dvTModel = matDerivEstadosModel(:,4);
dALPHATModel = matDerivEstadosModel(:,5);
ddPSIModel = matDerivEstadosModel(:,6);

ddXModel = dvTModel.*cos(PSIModel + ALPHATModel) - vTModel.*(dPSIModel + dALPHATModel).*sin(PSIModel + ALPHATModel);
ddYModel = dvTModel.*sin(PSIModel + ALPHATModel) + vTModel.*(dPSIModel + dALPHATModel).*cos(PSIModel + ALPHATModel);

ACELNumModel = [(ddXModel.*cos(PSIModel) - ddYModel.*sin(PSIModel))  (-ddXModel.*sin(PSIModel) + ddYModel.*cos(PSIModel))];

f7 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,ACELNumPlant(:,1),'r')
plot(TSPAN,ACELNumPlant(:,2),'g')
plot(TSPAN,ACELNumModel(:,1),'r--')
plot(TSPAN,ACELNumModel(:,2),'g--')
xlabel('time [s]')
ylabel('acc. [m/s]')
l = legend('AX Plant','AY Plant','AX Model','AY Model');
set(l,'Location','NorthEast')

%%
% <<../illustrations/plot/KalmanFilterFig7.svg>>
%

%%
% Comparing the maneuvers of the plant and the model.
%

set(gcf,'nextplot','replace')
gPlant.Frame();
hold on
gModel.Frame();

%%
% <<../illustrations/frame/KalmanFilterFrame3.svg>>
%
% *Model linearization*
%
% The general expression of the linearized model is optained using the symbolic processor of Octave/Matlab.
%
% Defining symbols
%

syms XT YT PSI vT ALPHAT dPSI mT IT a b K

%%
% Slip angles
%

ALPHAF = atan((vT * sin(ALPHAT) + a * dPSI)/(vT * cos(ALPHAT))); % Dianteiro
ALPHAR = atan((vT * sin(ALPHAT) - b * dPSI)/(vT * cos(ALPHAT))); % Traseiro

%%
% Lateral forces
%

FyF = -K*ALPHAF;
FyR = -K*ALPHAR;

%%
% State equations
%

f1e = vT * cos(ALPHAT + PSI);
f2e = vT * sin(ALPHAT + PSI);
f3e = dPSI;
f4e = (FyF * sin(ALPHAT) + FyR * sin(ALPHAT))/(mT);
f5e = (FyF * cos(ALPHAT) + FyR * cos(ALPHAT) - mT * vT * dPSI) / (mT * vT);
f6e = (FyF * a - FyR * b) / IT;

f = [f1e ; f2e ; f3e ; f4e ; f5e ; f6e];

%%
% State vector

States = [XT ; YT ; PSI ; vT ; ALPHAT ; dPSI];

%%
% The linearized system is written as
%
% $$ \dot{\bf x} = {\bf F} {\bf x} $$
%
% where ${\bf F}$ is the dynamic matrix of the linear model obtained after the truncation of the expansion of the Taylor series. Thus, the ${\bf F}$ matrix is
%
% $$ {\bf F} = \left[ \frac{\partial f_i}{\partial x_j} \right]_{n \times n} $$
%
% where $i$ e $j$ indicate the equations and state variables used in the calculations of at the position $(i,j)$ of the jacobian matrix.
%
% Calculating the jacobian matrix.
%

F = jacobian(f,States);
F = simplify(F);

%%
% Defining a matlab function to retrieve the numerical value of F. This way is faster than using "subs".
%

Ffunc = matlabFunction(F,'Vars',{PSI vT ALPHAT dPSI mT IT a b K});

%%
% *Measurements*
%
% Hypothetically, the measured quantities are:
%
% * Position X
% * Position Y
% * Yaw rate
% * Longitudinal acceleration
% * Lateral acceleration
%
% For this, we use:
%
% $$ \dot{x} = v_{\rm T} \cos \left( \psi + \alpha_{\rm T} \right) $$
%
% $$ \dot{y} = v_{\rm T} \sin \left( \psi + \alpha_{\rm T} \right) $$
%
% Longitudinal and lateral acceleration are
%
% $$ \ddot{x} = \dot{v}_{\rm T} \cos \left( \psi + \alpha_{\rm T} \right) - v_{\rm T} \left( \dot{\psi} + \dot{\alpha}_{\rm T} \right) \sin \left( \psi + \alpha_{\rm T} \right) $$
%
% $$ \ddot{y} = \dot{v}_{\rm T} \sin \left( \psi + \alpha_{\rm T} \right) + v_{\rm T} \left( \dot{\psi} + \dot{\alpha}_{\rm T} \right) \cos \left( \psi + \alpha_{\rm T} \right) $$
%
% Thus
%

ddX = f4*cos(PSI + ALPHAT) - vT*(dPSI + f5)*sin(PSI + ALPHAT);
ddY = f4*sin(PSI + ALPHAT) + vT*(dPSI + f5)*cos(PSI + ALPHAT);

%%
% The equations above describe the dynamics from $ \{ O {\bf i} {\bf j} {\bf k} \} $. In the moving reference frame $ \{ O {\bf t}_x {\bf t}_y {\bf t}_z \} $ the equations are
%
% $$ {\bf a} = \left( \ddot{x} \cos \psi - \ddot{y} \sin \psi \right) {\bf t}_x + \left( - \ddot{x} \sin \psi + \ddot{y} \sin \psi \right) {\bf t}_y $$
%
% Thus,
%

ACEL = [ddX*cos(PSI) - ddY*sin(PSI) ; -ddX*sin(PSI) + ddY*cos(PSI)];
ACEL = simplify(ACEL);

%%
% The nonlinear observation equation is given by
%
% $$ {\bf z}_k = {\bf h} ({\bf x}_k) + {\bf v}_k $$
%
% with $ {\bf v}_k $ ~ $ N ( {\bf 0} , {\bf R}_k ) $.
%
% The linear version is given by
%
% $$ {\bf z}_k = {\bf H} {\bf x}_k + {\bf v}_k $$
%
% where
%
% $$ {\bf H} = \left[ \frac{\partial h_i}{\partial x_j} \right]_{m \times n} $$
%
% that is, the output matrix {\bf H} is the jacobian matrix of the ACEL equation in relation to the system states.
%

medNonlinear = [XT ; YT ; f6 ; ACEL];

H = jacobian(medNonlinear,States);
H = simplify(H);

%%
% Again, defining matlab functions to retrieve the numerical value of medNonlinearfunc and H.
%

medNonlinearfunc = matlabFunction(medNonlinear,'Vars',{XT YT PSI vT ALPHAT dPSI mT IT a b K});

Hfunc = matlabFunction(H,'Vars',{PSI vT ALPHAT dPSI mT IT a b K});

%%
% *Extended Kalman Filter*
%
% Implementing the algorithm
%
% Noise matrix
%

G = eye(6); % Matriz identidade (6 x 6)

%%
% Covariance matrices
%

Q = eye(6);
R = eye(5);

%%
% Matrix

P0 = eye(6);


%%
% Retrieving the initial conditions defined above.
%

X0Num = simulatorModel.X0;
Y0Num = simulatorModel.Y0;
PSI0Num = simulatorModel.PSI0;
VEL0Num = simulatorModel.V0;
ALPHAT0Num = simulatorModel.ALPHAT0;
dPSI0Num = simulatorModel.dPSI0;

x0 = [ X0Num ; Y0Num ; PSI0Num ; VEL0Num ; ALPHAT0Num ; dPSI0Num ];
x0 = zeros(6,1);
x0(4)=20;

%%
% Retrieving the vehicle parameters.
%

mTNum = VehicleModel.mT;
ITNum = VehicleModel.IT;
aNum = VehicleModel.a;
bNum = VehicleModel.b;
KNum = TireModel.k;

parameters = [mTNum ITNum aNum bNum KNum];

%%
% Sample time
%

sampleTime = 0.1;

t = 0:sampleTime:T;                 % Array of observation instants

%%
% Preallocation
%

XOUTopt = zeros(length(t) + 1,length(States));  % Estimate of the states after update
Popt = zeros(length(t) + 1,1);                  % Covariance matrix after update
Pantes = zeros(length(t) + 1,1);                % Covariance matrix before update
KKalmanopt = ones(length(t) + 1,1);             % Kalman gain

%%
% Using the first values
%

XOUTopt(1,:) = x0';
XOUTantes(1,:) = x0';
Pantes(1,1) = norm(P0);
Popt(1,1) = norm(P0);

% Error distribution

pesos = [5; 5; 0.1; 0.5; 0.5];

%%
% *Iterations*
%

for j = 1:length(t)
    % Index varying through all instants of observation

    % Time span for the propagation phase
    tspan = t(j):sampleTime/100:t(j)+sampleTime;

    % Measures of the interation
    z = [interp1(TSPAN,XTPlant(:,1),t(j)) ;
         interp1(TSPAN,YTPlant(:,1),t(j)) ;
         interp1(TSPAN,ddPSIPlant(:,1),t(j)) ;
         interp1(TSPAN,ACELNumPlant(:,1),t(j)) ;
         interp1(TSPAN,ACELNumPlant(:,2),t(j))];

    z = z + pesos.*(rand(5,1)-0.5);

    Fnum = Ffunc(PSI0Num,VEL0Num,ALPHAT0Num,dPSI0Num,mTNum,ITNum,aNum,bNum,KNum);
    Hnum = Hfunc(PSI0Num,VEL0Num,ALPHAT0Num,dPSI0Num,mTNum,ITNum,aNum,bNum,KNum);

    % Propagation cycle
    % Transforming matrix PMat0 (6 x 6) in P0 (1 x 36)
    P0 = reshape(P0',[1 36]);

    [TOUT,Pout] = ode45(@(t,P) IntCov(t,P,Fnum,G,Q),tspan,P0);

    Pmatrix = reshape(Pout(end,:),[6 6])';

    simulatorKalman = Simulator(VehicleModel, tspan);

    % Initial conditions
    simulatorKalman.X0 = x0(1);
    simulatorKalman.Y0 = x0(2);
    simulatorKalman.PSI0 = x0(3);
    simulatorKalman.V0 = x0(4);
    simulatorKalman.ALPHAT0 = x0(5);
    simulatorKalman.dPSI0 = x0(6);

    % Simulation
    simulatorKalman.Simulate()

    XTKalman = simulatorKalman.XT;
    YTKalman = simulatorKalman.YT;
    PSIKalman = simulatorKalman.PSI;
    vTKalman = simulatorKalman.VEL;
    ALPHATKalman = simulatorKalman.ALPHAT;
    dPSIKalman = simulatorKalman.dPSI;

    XOUTKalman = [XTKalman YTKalman PSIKalman vTKalman ALPHATKalman dPSIKalman];

    % Update cycle

    ACELKalman = medNonlinearfunc(XTKalman(end),
                                  YTKalman(end),
                                  PSIKalman(end),
                                  vTKalman(end),
                                  ALPHATKalman(end),
                                  dPSIKalman(end),
                                  mTNum,
                                  ITNum,
                                  aNum,
                                  bNum,
                                  KNum);

    KKalman = Pmatrix*Hnum' / (Hnum*Pmatrix*Hnum' + R);

    XKalman = XOUTKalman(end,:)' + KKalman*(z - ACELKalman);
    PKalman = Pmatrix - KKalman*Hnum*Pmatrix;

    x0 = XKalman;
    P0 = PKalman;

    XOUTopt(j+1,:) = XKalman';
    XOUTantes(j+1,:) = XOUTKalman(end,:);
    Popt(j+1) = norm(PKalman);
    Pantes(j+1) = norm(Pmatrix);
    KKalmanopt(j+1) = norm(KKalman);

end

%%
% *Comparison*
%

f8 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,1),'r')
p = plot(t(2:end),XOUTopt(2:end-1,1),'r--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('x [m]')

f9 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,2),'g')
p = plot(t(2:end),XOUTopt(2:end-1,2),'g--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('y [m]')

f10 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,3),'b')
p = plot(t(2:end),XOUTopt(2:end-1,3),'b--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('PSI [rad]')

f11 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,4),'c')
p = plot(t(2:end),XOUTopt(2:end-1,4),'c--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('vT [m/s]')

f12 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,5),'m')
p = plot(t(2:end),XOUTopt(2:end-1,5),'m--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('ALPHAT [rad/s]')

f13 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,6),'k')
p = plot(t(2:end),XOUTopt(2:end-1,6),'k--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('dPSI [rad/s]')

% Cov. do erro
f14 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
p = plot(t(2:end),KKalmanopt(2:end-1),'r');
set(p,'Marker','*','MarkerSize',3)
ylabel('ganho de kalman')
xlabel('tempo [s]')

f15 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
p = plot(t(2:end),Popt(2:end-1),'r');
set(p,'Marker','*','MarkerSize',3)
p = plot(t(2:end),Pantes(2:end-1),'g');
set(p,'Marker','*','MarkerSize',3)
l = legend('+','-');
set(l,'Location','NorthEast')
ylabel('cov. erro')
xlabel('tempo [s]')

%%
% <<../illustrations/plot/KalmanFilterFig8.svg>>
%
% <<../illustrations/plot/KalmanFilterFig9.svg>>
%
% <<../illustrations/plot/KalmanFilterFig10.svg>>
%
% <<../illustrations/plot/KalmanFilterFig11.svg>>
%
% <<../illustrations/plot/KalmanFilterFig12.svg>>
%
% <<../illustrations/plot/KalmanFilterFig13.svg>>
%
% <<../illustrations/plot/KalmanFilterFig14.svg>>
%
% <<../illustrations/plot/KalmanFilterFig15.svg>>
%

%%
% *Estimated trajectory*
%
% Using simulatorPlant to initialize the Kalman frame.

set(gcf,'nextplot','replace')
gKalman = Graphics(simulatorKalman);
gKalman.Simulator.TSpan = t;
gKalman.Simulator.XT = XOUTopt(1:end-1,1);
gKalman.Simulator.YT = XOUTopt(1:end-1,2);
gKalman.Simulator.PSI = XOUTopt(1:end-1,3);
gKalman.Simulator.VEL = XOUTopt(1:end-1,4);
gKalman.Simulator.ALPHAT = XOUTopt(1:end-1,5);
gKalman.Simulator.dPSI = XOUTopt(1:end-1,6);
gKalman.TractorColor = 'b';

set(gcf,'nextplot','replace')
gPlant.Frame();
hold on
gKalman.Frame();

%%
% <<../illustrations/frame/KalmanFilterFrame4.svg>>
%
##### SOURCE END #####
-->
</body>
</html>
